{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridSmoother"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridSmoother.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridSmoother` implements an incremental fixed-lag smoother for hybrid systems. Unlike a full ISAM approach which keeps the entire history, a fixed-lag smoother marginalizes out older variables to maintain a bounded-size active window.\n",
        "\n",
        "It includes features for:\n",
        "*   Storing all factors (`HybridNonlinearFactorGraph`).\n",
        "*   Maintaining a linearization point (`Values`).\n",
        "*   Holding the current posterior (`HybridBayesNet`).\n",
        "*   Optionally removing \"dead modes\" based on a marginal probability threshold (`marginalThreshold_`).\n",
        "*   An `update` method to incorporate new factors.\n",
        "*   A `relinearize` method."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "from gtsam import (\n",
        "    HybridSmoother,\n",
        "    HybridNonlinearFactorGraph,\n",
        "    PriorFactorPose2, BetweenFactorPose2, Pose2, Point3,\n",
        "    DecisionTreeFactor, HybridNonlinearFactor,\n",
        "    Values\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Constructed optionally with a `marginalThreshold` for dead mode removal. It starts with an empty internal state."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initialized Smoother 1 (no threshold)\n",
            "Initialized Smoother 2 (threshold=0.99)\n",
            " Smoother 2 initial fixed values: DiscreteValues{}\n"
          ]
        }
      ],
      "source": [
        "# Initialize without dead mode removal\n",
        "smoother1 = gtsam.HybridSmoother()\n",
        "print(\"Initialized Smoother 1 (no threshold)\")\n",
        "\n",
        "# Initialize with dead mode removal threshold\n",
        "threshold = 0.99\n",
        "smoother2 = gtsam.HybridSmoother(marginalThreshold=threshold)\n",
        "print(f\"Initialized Smoother 2 (threshold={threshold})\")\n",
        "print(f\" Smoother 2 initial fixed values: {smoother2.fixedValues()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Update Steps\n",
        "\n",
        "The `update` method incorporates new nonlinear factors and initial estimates. It performs linearization, updates the internal `HybridBayesNet` posterior (by eliminating variables related to the new factors against the current posterior), and updates the linearization point. Pruning based on `maxNrLeaves` and `marginalThreshold` occurs during this step."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "--- Update 0 ---\n",
            "Smoother state after update 0:\n",
            " Lin Point Size: 1\n",
            " Factors Size: 2\n",
            " Posterior HybridBayesNet:\n",
            "HybridBayesNet\n",
            " \n",
            "size: 1\n",
            "conditional 0: p(x0)\n",
            "  R = [ 10  0  0 ]\n",
            "      [  0 10  0 ]\n",
            "      [  0  0 20 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  mean: 1 elements\n",
            "  x0: 0 0 0\n",
            "  logNormalizationConstant: 4.84409\n",
            "  No noise model\n",
            " Fixed Values: DiscreteValues{7205759403792793600: 0}\n",
            "\n",
            "--- Update 1 ---\n",
            "Smoother state after update 1:\n",
            " Lin Point Size: 2\n",
            " Posterior HybridBayesNet:\n",
            "HybridBayesNet\n",
            " \n",
            "size: 2\n",
            "conditional 0: p(x0 | x1)\n",
            "  R = [ 14.1421       0       0 ]\n",
            "      [       0 14.1421 7.07107 ]\n",
            "      [       0       0 61.0967 ]\n",
            "  S[x1] = [  -7.07107         0         0 ]\n",
            "          [         0  -7.07107         0 ]\n",
            "          [         0 -0.818375  -53.7313 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  logNormalizationConstant: 6.65396\n",
            "  No noise model\n",
            "conditional 1: p(x1)\n",
            "  R = [ 7.07107       0       0 ]\n",
            "      [       0 7.02355 -6.2607 ]\n",
            "      [       0       0 18.8827 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  mean: 1 elements\n",
            "  x1: 0 0 0\n",
            "  logNormalizationConstant: 4.08671\n",
            "  No noise model\n",
            " Fixed Values: DiscreteValues{7205759403792793600: 0}\n"
          ]
        }
      ],
      "source": [
        "smoother = HybridSmoother(marginalThreshold=0.99)\n",
        "\n",
        "# --- Initial Step (Pose 0 and Mode Prior) ---\n",
        "step0_graph = HybridNonlinearFactorGraph()\n",
        "step0_values = Values()\n",
        "dk0 = (D(0), 2)\n",
        "\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n",
        "step0_graph.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))\n",
        "step0_values.insert(X(0), Pose2(0.0, 0.0, 0.0)) # Initial estimate for X0\n",
        "\n",
        "step0_graph.add(DecisionTreeFactor([dk0], \"0.995 0.005\")) # High prior on D0=0\n",
        "\n",
        "print(\"--- Update 0 ---\")\n",
        "smoother.update(step0_graph, step0_values, maxNrLeaves=10)\n",
        "print(\"Smoother state after update 0:\")\n",
        "print(f\" Lin Point Size: {smoother.linearizationPoint().size()}\")\n",
        "print(f\" Factors Size: {smoother.allFactors().size()}\")\n",
        "print(\" Posterior HybridBayesNet:\")\n",
        "smoother.hybridBayesNet().print()\n",
        "print(f\" Fixed Values: {smoother.fixedValues()}\")\n",
        "\n",
        "\n",
        "# --- Second Step (Pose 1 and Hybrid Odometry) ---\n",
        "step1_graph = HybridNonlinearFactorGraph()\n",
        "step1_values = Values()\n",
        "\n",
        "noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))\n",
        "odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)\n",
        "noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))\n",
        "odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)\n",
        "hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])\n",
        "step1_graph.add(hybrid_odom)\n",
        "\n",
        "x0_estimate = smoother.linearizationPoint().atPose2(X(0))\n",
        "x1_initial_guess = x0_estimate.compose(Pose2(1.0, 0, 0))\n",
        "\n",
        "step1_values.insert(X(0), x0_estimate)\n",
        "step1_values.insert(X(1), x1_initial_guess)\n",
        "\n",
        "print(\"\\n--- Update 1 ---\")\n",
        "smoother.update(step1_graph, step1_values, maxNrLeaves=10)\n",
        "print(\"Smoother state after update 1:\")\n",
        "print(f\" Lin Point Size: {smoother.linearizationPoint().size()}\")\n",
        "print(\" Posterior HybridBayesNet:\")\n",
        "smoother.hybridBayesNet().print()\n",
        "print(f\" Fixed Values: {smoother.fixedValues()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Accessing State and Optimization\n",
        "\n",
        "Allows accessing the current linearization point, the posterior `HybridBayesNet`, and optimizing the posterior."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Current Linearization Point:\n",
            "Values with 2 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(0, 0, 0)\n",
            "\n",
            "Value x1: (gtsam::Pose2)\n",
            "(1, 0, 0)\n",
            "\n",
            "\n",
            "Current Posterior HybridBayesNet:\n",
            "HybridBayesNet\n",
            " \n",
            "size: 2\n",
            "conditional 0: p(x0 | x1)\n",
            "  R = [ 14.1421       0       0 ]\n",
            "      [       0 14.1421 7.07107 ]\n",
            "      [       0       0 61.0967 ]\n",
            "  S[x1] = [  -7.07107         0         0 ]\n",
            "          [         0  -7.07107         0 ]\n",
            "          [         0 -0.818375  -53.7313 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  logNormalizationConstant: 6.65396\n",
            "  No noise model\n",
            "conditional 1: p(x1)\n",
            "  R = [ 7.07107       0       0 ]\n",
            "      [       0 7.02355 -6.2607 ]\n",
            "      [       0       0 18.8827 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  mean: 1 elements\n",
            "  x1: 0 0 0\n",
            "  logNormalizationConstant: 4.08671\n",
            "  No noise model\n",
            "\n",
            "Current Fixed Values: DiscreteValues{7205759403792793600: 0}\n",
            "\n",
            "MAP Solution from Smoother:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  x0: 0 0 0\n",
            "  x1: 0 0 0\n",
            "  Discrete: (d0, 0)\n",
            "  Nonlinear\n",
            "Values with 0 values:\n",
            " Solution D(0): 0, Fixed D(0): 0\n"
          ]
        }
      ],
      "source": [
        "# Get the current linearization point\n",
        "lin_point = smoother.linearizationPoint()\n",
        "print(\"\\nCurrent Linearization Point:\")\n",
        "lin_point.print()\n",
        "\n",
        "# Get the posterior Bayes Net\n",
        "posterior_hbn = smoother.hybridBayesNet()\n",
        "print(\"\\nCurrent Posterior HybridBayesNet:\")\n",
        "posterior_hbn.print()\n",
        "\n",
        "# Get fixed values determined during updates\n",
        "fixed_vals = smoother.fixedValues()\n",
        "print(f\"\\nCurrent Fixed Values: {fixed_vals}\")\n",
        "\n",
        "# Optimize the current posterior Bayes Net (may implicitly use fixed_vals)\n",
        "map_solution = smoother.optimize()\n",
        "print(\"\\nMAP Solution from Smoother:\")\n",
        "map_solution.print()\n",
        "# Note: The solution should respect the fixed values.\n",
        "if D(0) in fixed_vals:\n",
        "     print(f\" Solution D(0): {map_solution.discrete()[D(0)]}, Fixed D(0): {fixed_vals[D(0)]}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Relinearization\n",
        "\n",
        "The `relinearize` method rebuilds the posterior `HybridBayesNet` by relinearizing all stored factors (`allFactors_`) around the current linearization point."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Relinearizing...\n",
            "Relinearization complete. Posterior HybridBayesNet:\n",
            "HybridBayesNet\n",
            " \n",
            "size: 2\n",
            "conditional 0: p(x1 | x0)\n",
            "  R = [      10       0       0 ]\n",
            "      [       0      10       0 ]\n",
            "      [       0       0 57.2958 ]\n",
            "  S[x0] = [      -10        0        0 ]\n",
            "          [        0      -10      -10 ]\n",
            "          [        0        0 -57.2958 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  logNormalizationConstant: 5.89658\n",
            "  No noise model\n",
            "conditional 1: p(x0)\n",
            "  R = [ 10  0  0 ]\n",
            "      [  0 10  0 ]\n",
            "      [  0  0 20 ]\n",
            "  d = [ 0 0 0 ]\n",
            "  mean: 1 elements\n",
            "  x0: 0 0 0\n",
            "  logNormalizationConstant: 4.84409\n",
            "  No noise model\n",
            "\n",
            "MAP Solution after relinearization:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  x0: 0 0 0\n",
            "  x1: 0 0 0\n",
            "  Discrete: (d0, 0)\n",
            "  Nonlinear\n",
            "Values with 0 values:\n"
          ]
        }
      ],
      "source": [
        "print(\"\\nRelinearizing...\")\n",
        "# This might be computationally expensive as it involves all factors\n",
        "try:\n",
        "    smoother.relinearize()\n",
        "    print(\"Relinearization complete. Posterior HybridBayesNet:\")\n",
        "    smoother.hybridBayesNet().print()\n",
        "    # Optimize again after relinearization\n",
        "    map_solution_relinearized = smoother.optimize()\n",
        "    print(\"\\nMAP Solution after relinearization:\")\n",
        "    map_solution_relinearized.print()\n",
        "except Exception as e:\n",
        "    print(f\"Relinearization failed: {e}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
